#include <iostream>
#include <string>
#include <vector>

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/point_cloud2_iterator.h>

#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/common/common.h>
#include <pcl_conversions/pcl_conversions.h>
#include <Eigen/Core>

#include "image_transport/image_transport.h"
#include "opencv2/core/core.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "cv_bridge/cv_bridge.h"
#include "sensor_msgs/image_encodings.h"

typedef pcl::PointXYZI PointType;

class FuseImg
{
public:
    FuseImg();
    ~FuseImg(){};

    void ImgCallback(const sensor_msgs::ImageConstPtr &msgImg);
    void PointCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg);

    ros::NodeHandle nh;
    ros::Subscriber subPointCloud;
    ros::Subscriber subImg;
    ros::Publisher pubFuseImg;

    Eigen::Matrix<double,4,4> transfromEigentoCamera;
    Eigen::Matrix<double,4,4> cameraPara;
    Eigen::Vector4d thispoint;
    pcl::PointCloud<PointType> pcl_cloud_;

    cv::Mat cvColorImgMat;
    sensor_msgs::ImagePtr fusemsg;
};

cv::Scalar fakecolor(int intensity);
